#!/usr/bin/env python3
#-*- coding: utf-8 -*-

import sys
import os
import re
import argparse
from enum import Enum

class OutFormat(Enum):
    CSV = 1
    PCD = 2

class Point:
    def __init__(self, x, y, z):
        self.__x = x
        self.__y = y
        self.__z = z

    def x(self):
        return self.__x

    def y(self):
        return self.__y

    def z(self):
        return self.__z

    def __str__(self):
        return f'{self.__x}, {self.__y}, {self.__z}'
    
class PointCloud:
    def __init__(self):
        self.__points = []

    def add_point(self, point):
        self.__points.append(point)

    def points(self):
        return self.__points

    def __str__(self):
        return '\n'.join([str(point) for point in self.__points])
    
    def save_csv(self, filename):
        with open(filename, 'w') as f:
            for point in self.__points:
                f.write(f'{point.x()}, {point.y()}, {point.z()}\n')
    
    def save_pcd(self, filename):
        with open(filename, 'w') as f:
            f.write('# .PCD v.7 - Point Cloud Data file format\n')
            f.write('VERSION .7\n')
            f.write('FIELDS x y z\n')
            f.write('SIZE 4 4 4\n')
            f.write('TYPE F F F\n')
            f.write('COUNT 1 1 1\n')
            f.write('WIDTH 1\n')
            f.write('HEIGHT 1\n')
            f.write('VIEWPOINT 0 0 0 1 0 0 0\n')
            f.write('POINTS 1\n')
            f.write('DATA ascii\n')
            for point in self.__points:
                f.write(f'{point.x()} {point.y()} {point.z()}\n')

class Splitter:
    def __init__(self, filename):
        self.__filename = filename
        self.__outpath = None
        self.__outformat = None
        self.__current_ts = 0
        self.__current_frame = 0

    def filename(self):
        return self.__filename
    
    def __parse_frame(self, lines):
        # get the timestamp
        ts_line = lines[0]
        # split the line by ,
        ts_parts = ts_line.split(',')
        # get the timestamp
        ts = int(ts_parts[1])
        # get data lines: the lines below the line including 'x(m),y(m),z(m)'
        for i in range(1, len(lines)):
            if 'x(m),y(m),z(m)' in lines[i]:
                data_lines = lines[i+1:]
                break
        # get the data: format: x, y, z
        point_cloud = PointCloud()
        for line in data_lines:
            parts = line.split(',')
            x = float(parts[0])
            y = float(parts[1])
            z = float(parts[2])
            point = Point(x, y, z)
            point_cloud.add_point(point)
        if ts != self.__current_ts:
            self.__current_ts = ts
            self.__current_frame = 0
        else:
            self.__current_frame += 1

    def parse(self, outpath, outformat):
        # outpath + filename as the output path
        self.__outpath = os.path.join(outpath, os.path.basename(self.__filename).split('.')[0])
        # create the output path if it does not exist
        if not os.path.exists(self.__outpath):
            os.makedirs(self.__outpath)
        # check the output format
        self.__outformat = outformat
        # spit the lines by empty lines
        with open(self.__filename, 'r') as f:
            lines = []
            for line in f:
                line = line.strip()
                if line:
                    lines.append(line)
                else:
                    self.__parse_frame(lines)
                    lines = []
                    # break
            if lines:
                self.__parse_frame(lines)
                

if __name__ == '__main__':
    # get the filename from the command line
    parser = argparse.ArgumentParser(description='Split a file into smaller files')
    parser.add_argument('-f', '--filename', help='the file to split', required=True)
    parser.add_argument('-o', '--outpath', default='.', help='the output path')
    parser.add_argument('-t', '--type', default='csv', help='the output file format')
    args = parser.parse_args()
    # get input filename
    filename = args.filename
    # get output path
    outpath = args.outpath
    # create a splitter object
    splitter = Splitter(filename)
    # parse the file
    splitter.parse(outpath, OutFormat.CSV if args.type == 'csv' else OutFormat.PCD)
    # done
    print('Done')
